/**
 ******************************************************************************
 * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
 * @file    Hardware_Node.h
 * @author
 * @brief
 ******************************************************************************
 */
#ifndef ROBOTRUNNER_H
#define ROBOTRUNNER_H

/* Includes ------------------------------------------------------------------*/
#include "common/Utilities/PeriodicTask.h"
#include "common/ControlParameters/RobotParameters.h"
#include "common/Controllers/LegController.h"
#include "common/Dynamics/Quadruped.h"
#include "common/Dynamics/Quadruped.h"
#include "common/ControlParameters/UserParameters.h"
#include "common/Controllers/DesiredStateCommand.h"
#include "common/Controllers/StateEstimatorContainer.h"
#include "robot/JPosInitializer.h"
#include "robot/RobotController.h"
#include "common/RobotCommand.h"

/* P1rivate macros ------------------------------------------------------------*/
/* Private type --------------------------------------------------------------*/
/* Exported function declarations --------------------------------------------*/
class RobotRunner {
    friend class Control_Node;
    friend class Route_Planning;
public:
    RobotRunner(RobotController*);
    void init(void);
    RobotCommand* run(void);

    // the interface of robot runner to exchange data with env
    void updateCommand(RobotCommand* cmd);
    void updateData(const LegData* data) { _legController->updateData(data); }
    void updataCheaterState(const CheaterState<float>* state);
    void updateVectorNavData(const VectorNavData* nav);
    void updateRC(const rc_control_settings* data);
    void updateRCvel(float vel_x, float vel_y, float omega);
    void updateRCgait(uint8_t gait);
    void cleanup(void);
    void initializeStateEstimator(bool cheaterMode = false);
    float dt(void){return controlParameters.controller_dt;}
    float* getLegdebugData(void){return _legController->debug_data;}

    virtual ~RobotRunner();

    RobotController* _robot_ctrl;


private:
    float _ini_yaw;

    int iter = 0;

    void setupStep();
    void finalizeStep();

    Quadruped<float> _quadruped;
    JPosInitializer<float>* _jpos_initializer;
    LegController<float>* _legController;
    StateEstimatorContainer<float>* _stateEstimator;
    DesiredStateCommand<float>* _desiredStateCommand;

    rc_control_settings rc_control;
    RobotCommand robot_cmd;
    RobotCommand last_cmd;
    RobotControlParameters controlParameters;
    StateEstimate<float> _stateEstimate;
    VectorNavData vectorNavData;
    CheaterState<float> cheaterState;
    // // Contact Estimator to calculate estimated forces and contacts
    FloatingBaseModel<float> _model;

    // test
    StateEstimatorContainer<float>* _stateEstimator_test;
    StateEstimate<float> _stateEstimate_test;
    bool _cheaterModeEnabled = false;
    u64 _iterations = 0;
    Leg_Mode ctrl_mode = Mode_Pos;
};

#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
